/*
 * FlightController_StickHandler.cpp
 *
 *  Created on: Jan 21, 2014
 *      Author: ppks
 */

#include "common.h"
#include "FlightController.h"
#include "FlightController_Configuration.h"
#include "FlightController_StickCmd.h"
#include "FlightController_Calibration.h"
#include "FlightController_PID.h"
#include "FlightController_Mode.h"
#include "GPS_common.h"

/******************************************************************************/
/* Local Variables Declaration                                                */
/******************************************************************************/
uint8_t  rcOptions[CHECKBOXITEMS];

/******************************************************************************/
/* Local Functions Declaration                                                */
/******************************************************************************/

/******************************************************************************/
/* Function Code                                                              */
/******************************************************************************/
/*******************************************************************************
* Function Name: FlightController_StickCommandHandler
********************************************************************************
* Summary:
*  This function checks the state of the AUX channels and executes commands
*  if the stick state was unchanged for specified amount of time
*  Supported commands:
*       THR_LO + YAW_LO + PIT_LO + ROL_CE : GYRO & BARO Calibration
*       THR_HI + YAW_LO + PIT_LO + ROL_CE : Accelerometer Calibration
*       THR_HI + YAW_HI + PIT_LO + ROL_CE : Magnetometer Calibration
*       THR_LO + YAW_LO + PIT_CE + ROL_CE : Disarm Option 1
*       THR_LO + YAW_CE + PIT_CE + ROL_LO : Disarm Option 2
*       THR_LO + YAW_HI + PIT_CE + ROL_CE : Arm Option 1
*       THR_LO + YAW_CE + PIT_CE + ROL_HI : Arm Option 2
*       THR_LO + YAW_LO + PIT_HI + ROL_HI : Enable In-flight Calibration
*       THR_HI + YAW_CE + PIT_HI + ROL_CE : PITCH Trim + 2
*       THR_HI + YAW_CE + PIT_LO + ROL_CE : PITCH Trim - 2
*       THR_HI + YAW_CE + PIT_CE + ROL_HI : ROLL Trim + 2
*       THR_HI + YAW_CE + PIT_CE + ROL_LO : ROLL Trim - 2
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
void FlightController_StickCommandHandler(void)
{
    static uint8_t rcDelayCommand;      /* this indicates the number of time (multiple of RC measurement at 50Hz)
                                           the sticks must be maintained to run or switch off motors */
    static uint8_t rcSticksState;       /* this hold sticks position for command combos */

    uint8_t rcCurrentStickState = 0;
    uint8_t i;

    /* Loop through all AUX channels and check MIN/MAX values */
    for (i = 0; i < NUMBER_OF_AUX_CHANNELS; i++)
    {
        rcCurrentStickState >>= 2;

        /* check for MIN */
        if (rcData[i] > RC_MINCHECK)
        {
            rcCurrentStickState |= 0x80;
        }

        /* check for MAX */
        if (rcData[i] < RC_MAXCHECK)
        {
            rcCurrentStickState |= 0x40;
        }
    }

    /* Check if previous state equal to a new stick state */
    if (rcCurrentStickState == rcSticksState)
    {
        if (rcDelayCommand < RC_DELAY_COUNTER_LIMIT)
        {
            rcDelayCommand++;
        }
    }
    else
    {
        rcDelayCommand = 0;
    }

    /* save the state of the sticks */
    rcSticksState = rcCurrentStickState;

    /* perform actions */
    if (rcData[THROTTLE] <= RC_MINCHECK)
    {
        /* If THROTTLE at minimum - reset errors for PID */
        errorGyroI[ROLL] = 0;
        errorGyroI[PITCH] = 0;
        errorGyroI_YAW = 0;
        errorAngleI[ROLL] = 0;
        errorAngleI[PITCH] = 0;

        if (FC_Config.BOX.activate[BOXARM] > 0)
        {
            /* Arming/Disarming via ARM BOX */
            if ((rcOptions[BOXARM]) && (globalFlags.OK_TO_ARM))
            {
                FlightController_SystemArm();
            }
            else if (globalFlags.ARMED)
            {
                FlightController_SystemDisarm();
            }
        }
    }

    /* Check if the sticks were RC_COMMAND_DELAY_CYCLES cycles in the same state */
    if (rcDelayCommand == RC_COMMAND_DELAY_CYCLES)
    {
        /* actions during armed */
        if (globalFlags.ARMED)
        {
#ifdef ALLOW_ARM_DISARM_VIA_TX_YAW
            if ((FC_Config.BOX.activate[BOXARM] == 0) && (rcSticksState == THR_LO + YAW_LO + PIT_CE + ROL_CE))
            {
                FlightController_SystemDisarm();    /* Disarm via YAW */
            }
#endif /* ALLOW_ARM_DISARM_VIA_TX_YAW */
#ifdef ALLOW_ARM_DISARM_VIA_TX_ROLL
            if ((FC_Config.BOX.activate[BOXARM] == 0) && (rcSticksState == THR_LO + YAW_CE + PIT_CE + ROL_LO))
            {
                FlightController_SystemDisarm();    /* Disarm via ROLL */
            }
#endif /* ALLOW_ARM_DISARM_VIA_TX_ROLL */
        }
        else
        {
            /* actions during not armed */
            i = 0;

            if (rcSticksState == THR_LO + YAW_LO + PIT_LO + ROL_CE)
            {
                /* GYRO calibration */
                calibration.GyroCycles = GYRO_CALIBRATION_CYCLES_DEFAULT;
#if GPS_INSTALLED
                /* Reset GPS Home Position */
                GPS_ResetHomePosition();
#endif /* GPS_INSTALLED */
#if BARO_INSTALLED
                /* calibrate baro to new ground level (10 * 25 ms = ~250 ms non blocking) */
                calibration.BaroCycles = 10;
#endif /* BARO_INSTALLED */
            }

#if defined(INFLIGHT_ACC_CALIBRATION)
            else if (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_HI)
            {
                /* Inflight ACC calibration START/STOP */
                if (AccInflightCalibrationMeasurementDone)
                {                /* trigger saving into eeprom after landing */
                    AccInflightCalibrationMeasurementDone = 0;
                    AccInflightCalibrationSavetoEEProm = 1;
                }
                else
                {
                    AccInflightCalibrationArmed = !AccInflightCalibrationArmed;
                }
            }
#endif /* defined(INFLIGHT_ACC_CALIBRATION) */

#ifdef ALLOW_ARM_DISARM_VIA_TX_YAW
            else if ((FC_Config.BOX.activate[BOXARM] == 0) && (rcSticksState == THR_LO + YAW_HI + PIT_CE + ROL_CE))
            {
                /* Arm via YAW */
                FlightController_SystemArm();
            }
#endif /* ALLOW_ARM_DISARM_VIA_TX_YAW */
#ifdef ALLOW_ARM_DISARM_VIA_TX_ROLL
            else if ((FC_Config.BOX.activate[BOXARM] == 0) && (rcSticksState == THR_LO + YAW_CE + PIT_CE + ROL_HI))
            {
                /* Arm via ROLL */
                FlightController_SystemArm();
            }
#endif /* ALLOW_ARM_DISARM_VIA_TX_ROLL */

#if ACC_INSTALLED
            else if (rcSticksState == THR_HI + YAW_LO + PIT_LO + ROL_CE)
            {
                calibration.AccCycles = ACC_CALIBRATION_CYCLES_DEFAULT;     /* throttle=max, yaw=left, pitch=min */
            }
#endif /* ACC_INSTALLED */
#if MAG_INSTALLED
            else if (rcSticksState == THR_HI + YAW_HI + PIT_LO + ROL_CE)
            {
                globalFlags.CALIBRATE_MAG = 1;  /* throttle=max, yaw=right, pitch=min */
            }
#endif /* MAG_INSTALLED */

            /* Check ROLL and PITCH Trim Commands */
            i = 0;
            if      (rcSticksState == THR_HI + YAW_CE + PIT_HI + ROL_CE) {FC_Config.RC.angleTrim[PITCH]+=2; i=1;}
            else if (rcSticksState == THR_HI + YAW_CE + PIT_LO + ROL_CE) {FC_Config.RC.angleTrim[PITCH]-=2; i=1;}
            else if (rcSticksState == THR_HI + YAW_CE + PIT_CE + ROL_HI) {FC_Config.RC.angleTrim[ROLL] +=2; i=1;}
            else if (rcSticksState == THR_HI + YAW_CE + PIT_CE + ROL_LO) {FC_Config.RC.angleTrim[ROLL] -=2; i=1;}

            if (i)
            {
                /* writeParams(1); TODO: write new parameters */
                rcDelayCommand = 0;    /* allow autorepetition */
            }
        }
    }

#if defined(INFLIGHT_ACC_CALIBRATION)
    if (AccInflightCalibrationArmed && globalFlags.ARMED && rcData[THROTTLE] > RC_MINCHECK && !rcOptions[BOXARM] )
    {
        /* Copter is airborne and you are turning it off via boxarm : start measurement */
        InflightcalibratingA = 50;
        AccInflightCalibrationArmed = 0;
    }

    if (rcOptions[BOXCALIB])
    {
        /* Use the Calib Option to activate : Calib = TRUE Meausrement started, Land and Calib = 0 measurement stored */
        if (!AccInflightCalibrationActive && !AccInflightCalibrationMeasurementDone)
        {
            InflightcalibratingA = 50;
        }
    }
    else if (AccInflightCalibrationMeasurementDone && !globalFlags.ARMED)
    {
        AccInflightCalibrationMeasurementDone = 0;
        AccInflightCalibrationSavetoEEProm = 1;
    }
#endif /* INFLIGHT_ACC_CALIBRATION */

    /**** Check AUX channels state ****/
#if defined(EXTENDED_AUX_STATES)
    /* Extended AUX states that allows 6 settings per each AUX channel */

    uint32_t auxState = 0;
    for (i = 0; i < NUMBER_OF_AUX_CHANNELS; i++)
    {
        auxState |= (rcData[AUX1 + i] <= AUX_CHANNEL_1_6_VALUE) << (6*i) |
                    (AUX_CHANNEL_1_6_VALUE < rcData[AUX1 + i] && rcData[AUX1+i] <= AUX_CHANNEL_2_6_VALUE) << (6 * i + 1) |
                    (AUX_CHANNEL_2_6_VALUE < rcData[AUX1 + i] && rcData[AUX1+i] <= AUX_CHANNEL_3_6_VALUE) << (6 * i + 2) |
                    (AUX_CHANNEL_3_6_VALUE < rcData[AUX1 + i] && rcData[AUX1+i] <= AUX_CHANNEL_4_6_VALUE) << (6 * i + 3) |
                    (AUX_CHANNEL_4_6_VALUE < rcData[AUX1 + i] && rcData[AUX1+i] <= AUX_CHANNEL_5_6_VALUE) << (6 * i + 4) |
                    (rcData[AUX1 + i] > AUX_CHANNEL_5_6_VALUE) << (6 * i + 5);
    }
#else /* defined(EXTENDED_AUX_STATES) */
    uint16_t auxState = 0;
    for (i = 0; i < NUMBER_OF_AUX_CHANNELS; i++)
    {
        auxState |= (rcData[AUX1 + i] <= AUX_CHANNEL_1_3_VALUE) << (3 * i) |
                    ((AUX_CHANNEL_1_3_VALUE < rcData[AUX1 + i]) && (rcData[AUX1 + i] <= AUX_CHANNEL_2_3_VALUE)) << (3 * i + 1) |
                    (rcData[AUX1 + i] > AUX_CHANNEL_2_3_VALUE) << (3 * i + 2);
    }
#endif /* defined(EXTENDED_AUX_STATES) */

    /* Determine rcOptions based on the aux state and selected configuration */
    for (i = 0; i < CHECKBOXITEMS; i++)
    {
        /* Changed to be ALL bits instead of ANY */
        rcOptions[i] = ((auxState & FC_Config.BOX.activate[i]) > 0);
    }

    /* Change Flight Mode if required */
    FlightController_ManageFlightMode();
}
